#include "SpeedAgent.h"

#include <cmath>
#include "MathUtility.h"

using namespace MathUtility;

SpeedAgent::SpeedAgent()
{
	for (int i = 0; i < 5; i++)
	{
		angles[i] = -90 + i*15;
		angles[18-i] = 90 - i*15;
	}
	
	for (int i = 5; i < 9; i++)
	{
		angles[i] = -20+(i-5)*5;
		angles[18-i] = 20-(i-5)*5;
	}
	//angles[8] = -0.5f;
	//angles[10] = 0.5f;
	angles[9] = 0;
	//angles[1] = -0.05f;
	//angles[17] = 0.05f;
	/*
	for (int i = 0; i < 19; i++)
	{
		//field[i].setAngle(convertDegToRad(field[i].getAngle()));
	}
	*/
}

SpeedAgent::~SpeedAgent()
{

}


void SpeedAgent::update(CarState& cs, const float dt)
{
	current = cs;
	field.clear();

	
	int pos = 0;
	// find the angle with highest position
	for (int i = 1; i < 19; i++)
	{
		if (cs.getTrack(i) > cs.getTrack(pos))
		{
			pos = i;
		}
	}
	
	Charge dir(0,convertDegToRad(angles[pos]));

	dir.setLength(speed);

	dir.update();
	insertCharge(dir, 0);
}

const float SpeedAgent::getRightFieldOfView()
{
	
	float velocity = current.getSpeedX() * VEL_KM_TO_M;
	
	if (velocity < current.getTrack(18))
		return MathUtility::pi*0.25f;
	//float total_length = current.getTrack(0) + current.getTrack(18);
	float length_to_right_wall = current.getTrack(18);

	return fabs(atanf((length_to_right_wall) / (velocity)));

}

const float SpeedAgent::getLeftFieldOfView()
{
	float velocity = current.getSpeedX() * VEL_KM_TO_M ;
	
	if (velocity < current.getTrack(0))
		return MathUtility::pi * 0.25f;
	//float total_length = current.getTrack(0) + current.getTrack(18);
	float length_to_left_wall = current.getTrack(0);
	
	return fabs(atanf((length_to_left_wall) / (velocity)));
}


void SpeedAgent::calculatePotentialForOptions(DriverOption* const options, unsigned int size)
{
	for (int i = 0; i < size; i++)
	{
		for (unsigned int j = 0; j < field.size(); j++)
		{
			float length = 0.0f;
			
			length = (options[i].getPosition() - field[j].getPosition()).length();
			
			//options[i].addPotential(30/sqrtf(length));
			
			options[i].addPotential(60 + (-1*pow(length,2.0f)*18));
		}
	}

}

void SpeedAgent::setSpeed(const float s)
{
	speed = s;
}

void SpeedAgent::setK(const float k)
{
	this->k = k;
}